/*
 * pid.c
 *
 *  Created on: Apr 2, 2024
 *      Author: boen
 */
#include <pid.h>


/**
* @brief
* @param
* @retval
*/
float pid_update(PID_t *self, float target)
{
//	self->err = self->SetTarget-target;
//	float inc = self->Kp*(self->err-self->err_next)+
//				self->Ki*self->err+
//				self->Kd*(self->err-2*self->err_next+self->err_last);
//	self->OutTarget += inc;
//	self->err_last = self->err_next;
//	self->err_next = self->err;

	self->err = self->SetTarget-target;
	self->OutTarget += self->Kp*(self->err-self->err_last) + self->Ki*self->err;
	self->err_last = self->err;

	//printf("pid err:%f next:%f out:%f \r\n",self->err, self->err_last, self->OutTarget);
	return self->OutTarget;
}


/**
* @brief
* @param
* @retval
*/
void pid_init(PID_t *self, float kp, float ki, float kd)
{
	self->SetTarget = 0.0f;
	self->OutTarget = 0.0f;
	self->err = 0.0f;
	self->err_last = 0.0f;
	self->err_next = 0.0f;
	self->Kp = kp;
	self->Ki = ki;
	self->Kd = kd;
}

